{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# BarometricFactor\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/BarometricFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
        "\n",
        "## Overview\n",
        "\n",
        "The `BarometricFactor` (contributed by [Peter Milani](https://github.com/pmmilani) in 2021) provides a way to incorporate altitude information derived from barometric pressure measurements into a GTSAM factor graph. It acts as a unary factor on a `Pose3` state variable but also connects to a `double` variable representing a slowly varying atmospheric pressure bias (or equivalently, an altitude offset).\n",
        "\n",
        "Barometers measure absolute atmospheric pressure. Under the assumption of a standard atmosphere model, this pressure can be converted into an estimate of altitude above sea level. However, local atmospheric pressure changes constantly due to weather, making the direct pressure-to-altitude conversion inaccurate over longer periods. The `BarometricFactor` accounts for this by simultaneously estimating the vehicle's altitude (Z-component of the `Pose3`) and a bias term that absorbs the slow variations in local atmospheric pressure relative to the standard model."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Mathematical Formulation\n",
        "\n",
        "### Measurement Model\n",
        "\n",
        "1.  **Pressure to Altitude:** The factor internally converts the input barometric pressure measurement $P_{meas}$ (in kPa) to an altitude estimate $h_{std}$ (in meters) using a standard atmosphere model (approximated by the factor's `heightOut` method, based on [NASA GRC](https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html)). This $h_{std}$ becomes the factor's internal measurement `nT_`.\n",
        "    $$ h_{std} = \\text{heightOut}(P_{meas}) $$ \n",
        "\n",
        "2.  **Factor Error:** The factor constrains the Z-component of the `Pose3` variable's translation ($p_z$) and the bias variable ($b$). The error is the difference between the altitude predicted by the state and bias, and the altitude derived from the measurement:\n",
        "    $$ e = (p_z + b) - h_{std} $$ \n",
        "    where:\n",
        "    - $p_z$ is the Z-coordinate of the `Pose3` translation.\n",
        "    - $b$ is the estimated altitude bias (in meters).\n",
        "    - $h_{std}$ is the altitude calculated from the pressure measurement.\n",
        "\n",
        "### Bias Modeling\n",
        "\n",
        "The bias $b$ is typically connected between successive time steps using a `BetweenFactor<double>` with a very small noise model, enforcing that the bias changes slowly over time (approximating a random walk)."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Key Functionality / API\n",
        "\n",
        "- **Constructor**: `BarometricFactor(poseKey, biasKey, baroIn_kPa, model)`: Creates the factor, taking the `Pose3` key, the `double` bias key, the pressure measurement *in kilopascals (kPa)*, and the 1D noise model for the altitude error.\n",
        "- **`evaluateError(pose, bias)`**: Calculates the 1D error $e = (pose.z() + bias) - h_{std}$.\n",
        "- **`measurementIn()`**: Returns the internally stored altitude measurement $h_{std}$ (converted from the input pressure).\n",
        "- **`heightOut(pressure_kPa)`**: Converts pressure (kPa) to altitude (m) using the standard atmosphere model.\n",
        "- **`baroOut(altitude_m)`**: Converts altitude (m) back to pressure (kPa) using the inverse model.\n",
        "- **`print` / `equals`**: Standard factor methods."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Usage Example\n",
        "\n",
        "Assume we have a pressure reading and want to add a factor constraining a `Pose3` and a bias."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Created BarometricFactor:\n",
            "Barometric Factor on x0Barometric Bias on b0\n",
            "  Baro measurement: 108.939\n",
            "  noise model: unit (1) \n",
            "\n",
            "Internal altitude measurement: 108.94 m\n",
            "Current Pose Z: 110.00 m\n",
            "Current Bias: -1.50 m\n",
            "Predicted Altitude (Pose Z + Bias): 108.50 m\n",
            "Error (Predicted - Measured): -0.44 m\n"
          ]
        }
      ],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam.symbol_shorthand import X, B # Pose key, Bias key\n",
        "\n",
        "# Define keys\n",
        "pose_key = X(0)\n",
        "bias_key = B(0)\n",
        "\n",
        "# Measurement\n",
        "pressure_kPa = 100.1 # Example pressure reading in kilopascals\n",
        "\n",
        "# Noise model on the *altitude* error (e.g., +/- 1 meter sigma)\n",
        "altitude_sigma = 1.0 \n",
        "noise_model = gtsam.noiseModel.Isotropic.Sigma(1, altitude_sigma)\n",
        "\n",
        "# Create the factor\n",
        "barometric_factor = gtsam.BarometricFactor(pose_key, bias_key, pressure_kPa, noise_model)\n",
        "\n",
        "print(\"Created BarometricFactor:\")\n",
        "barometric_factor.print()\n",
        "\n",
        "# Internal altitude measurement (converted from pressure)\n",
        "internal_altitude_measurement = barometric_factor.measurementIn()\n",
        "print(f\"\\nInternal altitude measurement: {internal_altitude_measurement:.2f} m\")\n",
        "\n",
        "# Example: Evaluate error \n",
        "# Assume current state estimate is Pose3 at z=110m and bias= -1.5m\n",
        "current_pose = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 110.0]))\n",
        "current_bias = -1.5\n",
        "\n",
        "error = barometric_factor.evaluateError(current_pose, current_bias)\n",
        "predicted_altitude = current_pose.z() + current_bias\n",
        "print(f\"Current Pose Z: {current_pose.z():.2f} m\")\n",
        "print(f\"Current Bias: {current_bias:.2f} m\")\n",
        "print(f\"Predicted Altitude (Pose Z + Bias): {predicted_altitude:.2f} m\")\n",
        "print(f\"Error (Predicted - Measured): {error[0]:.2f} m\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Source\n",
        "- [BarometricFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.h)\n",
        "- [BarometricFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.cpp)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}